
#include "linmin.h"
#include "polyfit.h"

#include <gsl/gsl_blas.h>

static const double chi = 9.0;
static const double mu = 0.01;
static const double eta = 0.1;
static const double tau = 0.05;

static const int I = 20;
static const int J = 10;

typedef struct
{
  double alphal, alphat, alphau;
  double phil, phit;
  double dphil, dphit;
  gsl_vector *xt;
  gsl_vector *gt;
}
fletcher_state_t;

static int fletcher_alloc(void *vstate, size_t n)
{
  fletcher_state_t *state = (fletcher_state_t *)vstate;
  
  state->xt = gsl_vector_alloc(n);
  state->gt = gsl_vector_alloc(n);
  
  return GSL_SUCCESS;
}

static void fletcher_free(void *vstate)
{
  fletcher_state_t *state = (fletcher_state_t *)vstate;
  
  gsl_vector_free(state->gt);
  gsl_vector_free(state->xt);
}

static double interp_safequard(double alpha, const fletcher_state_t *state)
{
  double delta_alpha = state->alphau - state->alphal;
	
  alpha = GSL_MAX_DBL(alpha, state->alphal + tau * delta_alpha);
  alpha = GSL_MIN_DBL(alpha, state->alphau - tau * delta_alpha);
	
  return alpha;
}

static double extrap_safequard(double alpha, const fletcher_state_t *state)
{
  double delta_alpha = state->alphat - state->alphal;
  double alpha_min = state->alphat + tau * delta_alpha;
  double alpha_max = state->alphat + chi * delta_alpha;
	
  alpha = GSL_MAX_DBL(alpha, alpha_min);
  alpha = GSL_MIN_DBL(alpha, alpha_max);
	
  alpha = GSL_MIN_DBL(alpha, state->alphat + 0.5 * (state->alphau - state->alphat));
	
  /*delta_alpha = alpha - state->alphat;
      
    delta_alpha = GSL_MAX_DBL(tau * (state->alphat - state->alphal), delta_alpha);
    delta_alpha = GSL_MIN_DBL(chi * (state->alphat - state->alphal), delta_alpha);
      
    delta_alpha = GSL_MIN_DBL(0.5 * (state->alphau - state->alphat), delta_alpha);
    alpha = state->alphat + delta_alpha;*/
	
  return alpha;
}

int fletcher_minimize(void *vstate, 
                      const lineminimizer_setup *setup, 
                      double alpha0, 
                      double f, 
                      double *alpha, 
                      gsl_vector *x_plus, 
                      double *f_plus, 
                      gsl_vector *g_plus)
{
  fletcher_state_t *state = (fletcher_state_t *)vstate;
  
  state->alphal = 0.0;
  state->alphau = 1e10;
  state->alphat = alpha0;
  
  int i, j;
  int status = GSL_SUCCESS;
	
  double alphat_plus;
  double phi0;
  double dphi0;
	
  phi0 = f;
  gsl_blas_ddot(setup->g, setup->d, &dphi0);
	
  state->phil = f;
  state->dphil = dphi0;
	
  j = 0;
  do
  {
    gsl_blas_dcopy(setup->x, state->xt);
    gsl_blas_daxpy(state->alphat, setup->d, state->xt);
    state->phit = GSL_MULTIMIN_FN_EVAL_F(setup->fdf, state->xt);
    
    i = 0;
    while(state->phit > phi0 + mu * state->alphat * dphi0)
    {
      status = quad_minimizer1(state->alphal, state->phil, state->dphil, state->alphat, state->phit, &alphat_plus);
      if(status != GSL_SUCCESS)
        alphat_plus = state->alphal + 0.5 * (state->alphat - state->alphal);
      alphat_plus = interp_safequard(alphat_plus, state);
      
      state->alphau = state->alphat;
      state->alphat = alphat_plus;
      
      gsl_blas_dcopy(setup->x, state->xt);
      gsl_blas_daxpy(state->alphat, setup->d, state->xt);
      state->phit = GSL_MULTIMIN_FN_EVAL_F(setup->fdf, state->xt);
      
      i++;
      if(i == I)
        break;
    }
    
    GSL_MULTIMIN_FN_EVAL_DF(setup->fdf, state->xt, state->gt);
    gsl_blas_ddot(state->gt, setup->d, &state->dphit);
    
    if(j == J || fabs(state->alphal - state->alphau) < 10.0 * GSL_DBL_EPSILON)
      break;
    
    if(state->dphit < eta * dphi0)
    {
      status = quad_minimizer2(state->alphal, state->dphil, state->alphat, state->dphit, &alphat_plus);
      if(status != GSL_SUCCESS)
        alphat_plus = 0.5 * (state->alphau + state->alphat);
      else   
        alphat_plus = extrap_safequard(alphat_plus, state);
      
      state->alphal = state->alphat;
      state->phil = state->phit;
      state->dphil = state->dphit;
      state->alphat = alphat_plus;
    }
    else
      break;
    
    j++;
  }
  while(1);
  
  *alpha = state->alphat;
  gsl_blas_dcopy(state->xt, x_plus);
  *f_plus = state->phit;
  gsl_blas_dcopy(state->gt, g_plus);
	
  return GSL_SUCCESS;
}

static const lineminimizer_type fletcher_type = 
{
  "fletcher",
  sizeof(fletcher_state_t),
  &fletcher_alloc,
  &fletcher_minimize,
  &fletcher_free
};

const lineminimizer_type *lineminimizer_fletcher = &fletcher_type;
